#pragma once

#include <vector>

#include "../geometry/i_homography.h"
#include "../geometry/i_plane.h"
#include "../da/i_ransac.h"

namespace idl
{
  /*This function estimates at most K planes given the depthmap of the reference view and camera poses of two cameras
    using Ransac. It works by iteratively estimae homographies from 2D-2D correspondences.
    pixel labels (1,2,...,K) are stored in buffer label*/
  template<typename T, int K>
  int stereoPlanarClustering(const T *const *xs,
                             const T *const *xps,
                             int n,
                             const T K1[9], const T R1[9], const T t1[3],
                             const T K2[9], const T R2[9], const T t2[3],
                             int &actual_nr_planes,
                             T **planes, /*plane equations[K][4]*/
                             int *label,     /*length n array*/
                             T homography_2d_warp_tol,
                             double confidence = 0.99,
                             double inlierprob = 0.25) /*the maximum allowed homography based warping error in pixel*/
  {
    int i, i3, j, j3, nr_inliers = 0, iter = 0, plane_id;
    int nr_samples_left = n;
    bool termination = false;
    T H[9];
    //double inlier_prob;
    actual_nr_planes = 0;

    if (!xs || !xps || !K1 || !R1 || !t1 || !K2 || !R2 || !t2 || !planes || !label)
    {
      return -1; /*error: invalid input*/
    }
    /*clear the label buffer*/
    i_zero(label, n);
    /*clear plane equations*/
    i_zero(planes[0], 4*K);

    std::vector<T>  x;
    std::vector<T> xp;
    std::vector<int> inliers;
    std::vector<int> inliers_index;
    while (iter < K && !termination)
    {
      plane_id = iter + 1; /*plane id starts from 1*/
      //inlier_prob = i_max(0.25, i_rec(K - actual_nr_planes + 1)); //i_div(i_div((double)nr_samples_left, (K - actual_nr_planes)), nr_samples_left);
      //printf("inlier prob: %lf\n", inlier_prob);
      x.resize(nr_samples_left  * 3);
      xp.resize(nr_samples_left * 3);
      inliers.resize(nr_samples_left * 2);
      inliers_index.resize(nr_samples_left);
      for (i = 0, i3 = 0, j = 0, j3=0; i < n; ++i, i3+=3)
      {
        if (!label[i])
        {
          i_copy3(xs[0]  + i3,  x.data() + j3);
          i_copy3(xps[0] + i3, xp.data() + j3);
          inliers_index[j++] = i;
          j3 += 3;
        }
      }
      printf("# of Ransac smaples %d\n", nr_samples_left);
      if (robustBinaryFitRansac<T, 3, 3, 9, 4, i_four_point_homography_ransac_hypogenfunc, i_homography_ransac_oneview_costfunc, i_plane_fit_total_least_square>(x.data(), xp.data(), nr_samples_left, H, nr_inliers, inliers.data(), homography_2d_warp_tol, true, false, confidence, inlierprob))
      {
        /*update labels*/
        printf("# of inliers Ransac iter %d : %d\n", iter, nr_inliers);
        for (i = 0; i < nr_inliers; ++i)
        {
          j = inliers[i];
          label[inliers_index[j]] = plane_id;
          nr_samples_left--;
        }
        actual_nr_planes++;
      }
      else
      {
        termination = true;
      }
      if (actual_nr_planes == K)
      {
        termination = true;
      }
      ++iter;
    }
    return actual_nr_planes;
  }

  /*This function estimates at most K planes given the depthmap of the reference view using Ransac. 
  It works by iteratively estimae planes from 3D point clouds.
  point labels (1,2,...,K) are stored in buffer label*/
  template<typename T, int K>
  int monocularPlanarClustering(const T *const *threeds, /*nx3 3D points*/
                                int n, /*# of 3D points*/
                                int &actual_nr_planes,
                                T **planes, /*plane equations[K][4]*/
                                int *label, /*length n array*/
                                const T point_to_plane_dist_tol,
                                const double confidence = 0.99,
                                const double inlierprob = 0.25,
                                const int min_cluster_size = 4)
  {
    int i, j, j3, nr_inliers = 0, iter = 0, plane_id;
    
    int nr_samples_left = n;
    bool termination = false;
    T Pi[4];
    actual_nr_planes = 0;

    if (!threeds || !planes || !label)
    {
      return -1; /*error: invalid input*/
    }
    /*clear the label buffer*/
    i_zero(label, n);
    /*clear plane equations*/
    i_zero(planes[0], 4 * K);

    std::vector<T> X;
    std::vector<int> inliers;
    std::vector<int> inliers_index;
    while (iter < K && !termination)
    {
      plane_id = iter + 1; /*plane id starts from 1*/
      //inlier_prob = i_max(0.25, i_rec(K - actual_nr_planes + 1)); //i_div(i_div((double)nr_samples_left, (K - actual_nr_planes)), nr_samples_left);
      //printf("inlier prob: %lf\n", inlier_prob);
      X.resize(nr_samples_left * 3);
      inliers.resize(nr_samples_left * 2);
      inliers_index.resize(nr_samples_left);
      for (i = 0, j = 0, j3 = 0; i < n; ++i)
      {
        if (!label[i])
        {
          i_copy3(threeds[i], X.data() + j3);
          inliers_index[j++] = i;
          j3 += 3;
        }
      }
      printf("# of Ransac smaples %d\n", nr_samples_left);
      if (robustFitRansac<T, 3, 4, 3, i_plane_fit, i_plane_fit_ransac_costfunc, NULL>(X.data(), nr_samples_left, Pi, nr_inliers, inliers.data(), point_to_plane_dist_tol, true, false, confidence, inlierprob, min_cluster_size))
      {
        /*update labels*/
        printf("# of inliers Ransac iter %d : %d\n", iter, nr_inliers);
        for (i = 0; i < nr_inliers; ++i)
        {
          j = inliers[i];
          label[inliers_index[j]] = plane_id;
          nr_samples_left--;
        }
        i_copy4(Pi, planes[iter]);
        actual_nr_planes++;
      }
      else
      {
        termination = true;
      }
      if (actual_nr_planes == K)
      {
        termination = true;
      }
      ++iter;
    }
    return actual_nr_planes;
  }

  /*This function estimates at most K planes given the depthmap of the reference view using Ransac.
  It works by iteratively estimae planes 3D point clouds, the main fitting parameter - "point_to_plane_dist_tol" is updated adaptively.
  point labels (1,2,...,K) are stored in buffer label*/
  template<typename T, int K>
  int monocularPlanarClustering(const T *const *threeds, /*nx3 3D points*/
                                int n, /*# of 3D points*/
                                int &actual_nr_planes,
                                PlanePara<T> planes[K], /*plane equations[K]*/
                                int *label, /*length n array*/
                                const T point_to_plane_dist_tol,
                                const double confidence = 0.99,
                                const double inlierprob = 0.25,
                                const int min_cluster_size = 4)
  {
    int i, j, j3, nr_inliers = 0, iter = 0, plane_id;
    T point_to_plane_dist_tol_iter = point_to_plane_dist_tol;
    int nr_samples_left = n;
    bool termination = false;
    T Pi[9];
    actual_nr_planes = 0;

    if (!threeds || !planes || !label)
    {
      return -1; /*error: invalid input*/
    }
    /*clear the label buffer*/
    i_zero(label, n);
    /*clear plane equations*/
    for (i = 0; i < K; ++i)
    {
      planes[i].clear();
    }
    std::vector<T> X;
    std::vector<int> inliers;
    std::vector<int> inliers_index;
    while (iter < K && !termination)
    {
      plane_id = iter + 1; /*plane id starts from 1*/
      X.resize(nr_samples_left * 3);
      inliers.resize(nr_samples_left * 2);
      inliers_index.resize(nr_samples_left);
      for (i = 0, j = 0, j3 = 0; i < n; ++i)
      {
        if (!label[i])
        {
          i_copy3(threeds[i], X.data() + j3);
          inliers_index[j++] = i;
          j3 += 3;
        }
      }
      printf("# of Ransac samples %d\n", nr_samples_left);
      printf("Fitting threshold: <%lf>\n", point_to_plane_dist_tol_iter);
      if (robustFitRansac<T, 3, 9, 3, i_plane_fit_adv, i_plane_fit_ransac_costfunc, i_plane_fit_total_least_square_adv>(X.data(), nr_samples_left, Pi, nr_inliers, inliers.data(), point_to_plane_dist_tol_iter, true, false, confidence, inlierprob, min_cluster_size))
      {
        /*update labels*/
        printf("# of inliers Ransac iter %d : %d\n", iter, nr_inliers);
        for (i = 0; i < nr_inliers; ++i)
        {
          j = inliers[i];
          label[inliers_index[j]] = plane_id;
          nr_samples_left--;
        }
        planes[iter].assign(Pi);
        point_to_plane_dist_tol_iter = i_average(point_to_plane_dist_tol, planes[iter].data_stat[0] + 3 * planes[iter].data_stat[1]); /*update fitting parameters: mean + 3sigma*/
        //printf("Mean fitting error & sdv: <%lf, %lf>\n", planes[iter].data_stat[0], planes[iter].data_stat[1]);
        actual_nr_planes++;
      }
      else
      {
        termination = true;
      }
      if (actual_nr_planes == K)
      {
        termination = true;
      }
      ++iter;
    }
    return actual_nr_planes;
  }
  
 

}/* namespace idl */